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Initialization  scheme  for  finding  a  point  of  coincidence 
E"*"  5  «>***  internal  coordinate  system  and  the  coordinate  system  of  Its  work 
Ec!*  °Vr  "fthod  uses  an  optical  transmitter-receiver  pair  mounted  on  the  robot 

scf!L5  J~,h*P*d  Plu*r  target  mounted  on  the  work  surface.  An  auto- 
^or  moving  a  point  on  the  end  effector  Into  precise  cone  1  dene 
ith  the  Point  of  intersection  between  a  sensed  plane  parallel  to  the  work  surface 
m  a  sensed  line  normal  to  the  work  surface  Is  shown  to  converge  rapidly. 


lfied 


i  ? 

Table  of  Contents 


2  ianodnctka 
3Coaflpntioa 


iLMMlm 

TCobUh 


ss  ssaaa 


Ustof  Hourss 


Heart  t:  T-shaped  tarfet 

Heart  2;  Tnwumioer  receiver  pair  and  detection  dreak 
H(trt  3:  Slept  1  (shooJdcr-fbrward)  and  2  (dbowHbrward) 
Heart  4:  Step  3  (shoulder  and  dhow  back-rotation) 

Heart  5:  Error  cawed  by  inexact  placement  of  the  target 

Heart  fc  Y 

Heart  7:  ERJtOltfB,) 

Heart  k  HRKOlUBp 
Heart*  ERKOtfK) 

Heart  Ik  EMORfD) 

Heart  11:  Robot  aran  with  arift 


1 


/ 


Abstract 


We  deocribe  m  aotonsadc  inirtalirtoion  acbcme  for  fading  a  point  of  coincidence  betwoen  a  robot's 
fotwnai  caordi— te  paw  and  foe  coowHaasc  ayatotn  of  to  wort  apace.  Our  mcfood  uses  an  optical 
tmaanfacrtocoivor  pair  Mounted  on  foe  robot  cod  effector  10  aeon  a  T*sbapod  pianar  target  mounted  oo 
die  work  aniftce.  An  aMomaaed  baraeto  method  for  moving  a  point  an  die  aid  cfector  into  pndee 
coincidencr  wbb  foe  point  of  foaanwion  between  aaenaed  plane  perafcl  to  foe  walk  turfooe  and  a 
aenaod  line  normal  lo  foe  work  nirfoce  b  foown  to  converge  rapidly. 
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2  Introduction 


Boca  ample  items  are  greenly  able  10  execute  differential  modotts  with  reasonable  accuracy,  and  hitft 
quality  robots  com  equipped  with  absolute  encoders  that  enable  them  to  discern  their  pose.  Rut  even 
high  quality  robots  are  generally  oblivious  to  how  their  internal  coordinate  system  maps  onto  the 
coordinate  system  of  their  work  space,  la  Aon.  even  die  best  robots,  which  run  closed  loop 
Hncuhmkidy.  generally  ran  open  loop  with  respect  to  the  coordinate  systems  of  their  working 


But  provided  that  the  distance  and  angle  measuring  systems  of  both  the  robot  and  the  work  space  are 
themselves  adequate,  hading  a  single  point  of  coincidence  between  the  two  coordinate  systems  is 
sufficient  to  ewablMi  the  mapping  between  them.1  For  example,  the  instruction  manual  for  the 
M1CROBOT  TeacfaMovcr[2]  robot  on  which  wc  implemented  a  demonstration  of  our  procedure 
suggests  an  operator  run  biWalixation  procedure  in  which  the  robot  arm  is  located  on  a  ooe-square-per- 
ineb  cancsisn  grid,  and  the  end  effector  is  moved  (using  the  teach  pendant)  into  a  specified  poae  above  a 
pate  in  this  space.  The  method  is  simple,  but  k  is  tedious,  inaccurate,  and  prone  to  operator  errors  and 


This  pqmr  describes  a  simple  and  precise  sensor  based  automatic  initialization  method  using  a  planar 
ictBMd  by  optics!  ycfjtttks  sensor.  We  developed  a  demonstration  of  this  system  specifically 
fer  an  srticiilseud  arm  robot  with  waist,  shoulder,  and  elbow  degrees  of  freedom,  but  our  method  is 
generally  applicable  to  any  robot  with  similarly  implemented  degrees  of  freedom,  and  the  principles  ate 
easily  adapted  to  robots  with  other  degrees  of  freedom  and  correspondingly  different  internal  coordinate 


Dealing  automatically  with  degrees  of  freedom  beyond  the  three  consisting  of  waist,  shoulder,  and  elbow 
will  require  additional  sensor  capabilities,  but  the  touted  system  described  here  can  still  be  retained  as  a 
subset  Since  the  robot  we  used  for  the  demonstration  also  has  wrist  angle,  wrist  rotation,  and  gripper 
doaure  degrees  of  freedom,  we  have  to  specify  how  we  win  remove  the  degeneracy  associated  with  these 
degrees  of  freedom.  The  method  may  be  manual  or  sensor  based  and  automatic.  In  our  demonstration, 
we  resolve  the  degeneracy  by  a  manual  pre-inidalizack»  wherein  we  adjust  the  plane  of  die  gripper 
action  to  be  tangent  to  a  cylinder  concentric  with  the  waist  rotation  axis,  we  cloee  the  gripper  fingers  to  a 
tendmd  gap.  end  we  uae  the  coordinating  features  of  the  robot  controller  firmware  to  preserve  these 


Our  mama  hoe  two  hardware  components: 

L  a  planar  iwget  paineed  on  or  attached  to  the  work  surfecr,  and 
2.  an  optical  man tear  racsivarpte  operand  as  a  proaimky  mm 


Vocinw  mpe  aoovc  bn  wont  sutmM» 


(MilHifrapM* 
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1  guide  the  end  effector  rcproducibly  to  positions  directly  above  transitions  between  target  and 
wort  surftce; 

3.  accomplish  the  latter  robustly  over  at  least  a  small  range  of  heights. 

In  our  implementation  the  sensor  is  an  optical  proximity  sensor,  the  wort  surtax  is  white,  and  the  target 
is  Mack.  However  there  is  no  obstacle  to  implementing  this  system  with  other  targets  and  target  detection 
Kncwci,  noc  noccsiiniy  opocn 


3  Configuration 


3.1  Targnt 

The  target,  affixed  to  die  work  surface,  is  a  T-shaped  pattern  with  a  body  that  is  part  of  a  sector  of  a  circle 
centered  on  the  waist  notation  axis,  and  with  a  rectangular  head,  as  shown  in  Figure  1.  In  the  first  part  of 
the  initialization  procedure,  the  sensor  directs  initialization  of  the  waist  joint  by  finding  the  radial  line  A 
in  Figure  1.  In  the  second  pan  of  the  procedure,  the  sensor  directs  initialization  of  the  shoulder  and 
elbow  joints  by  iteratively  *djiwting  them  while  repetitively  finding  the  tangent  line  B  in  Figure  L  It  is 
most  convenient  to  perform  die  shoulder  and  elbow  initialization  at  a  fixed  waist  joint  angular  offset  from 
the  waist  joint  initialization  position. 
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3.2  Sonoor 

In  our  demonstration  system  the  sensor  is  a  commercial  optical  proximity  sensor  [1]  consisting  of  an  LED 
integrally  packaged  with  a  photodctector,  and  with  simple  lenses  whose  optical  axes  intersect  about  a 
centimeter  from  the  package.  When  the  outgoing  infra-red  light  beam  meets  a  morc-or-lcss  white 
material  at  about  a  centimeter,  sufficient  radiation  from  the  LED  is  reflected  back  to  the  phototransistor 
to  fire  the  schmidt  trigger  output  circuit  (sec  Figure  2).  Since  die  precise  triggering  distance  from  the 
target  is  sensitive  to  the  target  reflectivity,  to  obtain  high  reproducibility  it  is  necessary  to  standardize  the 
background  reflectivity  immediately  adjacent  to  the  target  This  is  easily  accomplished  by  integrating  a 
small  patch  of  standard  background  with  the  target  by  painting  the  target  on  a  rectangle  of  adhesive 
paper  of  reproducible  reflectivity. 


FigH*2:  Transmitter-receiver  pair  and  detection  circuit 


3.3  Initialization  Proeoduro 

The  robot  is  powered  up.  and  moved  by  means  of  the  teach  pendant  to  any  position  new  the  intersection 
of  tines  A  and  B  in  Figure  1,  ia  the  ooe  work  surface  quadrant  they  define,  and  between  one  and  five 
centimeters  above  the  work  suffice.  A  suitable  starting  point  could,  of  course,  be  found  automatically  by 
exhaustive  blind  search,  but  there  would  be  little  or  no  practical  value  in  doing  to. 

Automated  procedures  tor  initializing  the  robot  waist,  tiwulder,  and  elbow  joints  an  then  called.  The 
sequence  of  events  is  outlined  in  the  following  sections. 

3.3.1  Wntet  Joint  Initialization 

Simple,  narrow  search  algorithms  effect  the  following  motions: 

1.  The  shoulder  is  rotated  toward  (be  work  surface  until  the  proximity  sensor  flies,  at 
approximately  one  centimeter  above  the  surface; 

2.  Shoulder  rotations  are  stopped,  and  the  waist  joint  is  rotated  until  the  radial  line  A  is  found 
(Flfore  1). 
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The  waist  joint  is  thus  initialized  The  waist  is  then  offset  by  a  few  degrees,  putting  the  sensor  back  above 
the  work  surface  in  the  quadrant  determined  by  lines  A  and  B. 

3.3.2  SlKMiidwr  and  Elbow  Joint  Initialization 

This  is  the  most  difficult  part:  before  initialization,  it  is  impossible  to  move  the  end  effector  without  also 
changing  its  height  above  the  work  surface.  One  approach  is  to  oscillate  about  the  desired  height  by 
using  sensor  feedback  to  control  alternate  very  small  shoulder  and  elbow  motions  while  we  search  for  line 
B.  What  we  now  show  is  dun  a  sequence  of  much  larger  sequential  shoulder  and  elbow  motions  will 
accomplish  the  same  end  much  more  efficiently. 


Pignrtl  Sseps  1  (shoulder-forward)  and  2  (elbow- forward) 


1.  The  dioulder  is  rotated  toward  the  work  surface  until  the  proximity  sensor  fires,  as  shown  in 
Figure  3,  path  W(  to  W^.  This  !s  a  "pure  shoulder  rotation,"  Lei,  the  robot  is  presumed  to  be 
equipped  with  well  calibrated  firmware  to  cause  elbow  rotation  equal  and  opposite  to  the 
shoulder  rotation,  thus  preserving  the  orientation  of  HnkL^  in  the  workspace. 

1  The  dhow  is  rotated  to  approach  fine  B.  which  is  detected  by  the  proximity  sensor  seeing  the 
dark  target  in  contrast  to  the  bright  work  surface,  as  shown  in  Figure  3,  path  WH  to  W(.  This 
is  a  "pure  elbow  rotation”.  ie,  the  gripper  (not  shown  in  the  figure)  remains  in  the  vertical 
orientation  it  was  given  in  the  pre-initialization  procedure  discussed  above. 
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3.  Both  the  shoulder  and  elbow  arc  rotated  back  through  arbitrary  small  anglcs(Ar  and  B^  as 

shown  in  Figure.  4  path  Wg  u>  w/1*.  While  the  choice  of  Af  and  Bf  is  arbitrary,  initialization 
accuracy  is  shown  in  Figure  8  to  depend  somewhat  on  the  value  chosen  for  Bf.  Furthermore, 
the  algorithm  assumes  that  once  the  choices  arc  made,  they  are  kept  constant  throughout  the 
procedure. 

4.  The  cycle  (steps  1, 2, 3)  is  repeated  several  times,  according  to  the  precision  required. 

5.  After  a  predetermined  number  of  iterations,  the  procedure  is-  terminated  after  step  2. 


Fifuc4:  Step  3  (shoulder  and  elbow  back-rotation) 

It  is  important  to  notice  that  step  3  must  be  a  compound  motion  involving  back  rotation  of  both  the 
shoulder  and  elbow  joints.  Neither  a  pure  shoulder  motion  nor  a  pure  elbow  motion  could  guarantee 
convergence  of  foe  iteration.  The  analysis  in  Section  3  derives  the  relationships  that  determine  the 
accuracy  of  foe  initialization  procedure  as  a  function  of  the  number  of  iterations  and  the  size  of  the  back 
rotations  taken  in  step  3.  The  theoretical  analysis,  well  confirmed  by  our  experiments,  shows  that  two  or 
force  iterations  are  sufficient  to  achieve  initialization  accuracy  limited  by  foe  mechanics  of  foe  refoot  In 
table  2*1  we  illustrate  foe  rapid  convergence  from  an  arbitrary  pre-initialization  position. 


TABLE  2-1 


Vertical  Initialization  Error  vs.  Procedure  Cycles 


(Bi*290  Br*5  K*140  tm  D* 117  mm  ) 


cycles 

0 

1 

2 

3 

4 

5 

8 

error 

(micron) 

-10138 

178.2 

-5.635 

0.1788 

-3 

-5.5x10 

-4 

1.7x10 

-« 

-5.8x10 

4  Initialization  Accuracy  Analysis 

The  accuracy  analysis  of  the  waist  initialization  is  very  simple.  The  only  source  of  error2  is  inexact 
placement  of  the  target 

Suppose  that  the  distance  between  the  working  side  of  the  body  of  the  target  and  the  axis  of  rotation  of 
the  base  joint  is  e,  where  nominally,  esfl  (Figure  S).  The  range  of  hand  starting  positions  is  from  Rx  to 
R?  corresponding  to  a  waist  initialization  angular  error  range  AC  where 

AC  a  C2  -  q  =  cos'VR2)  •  cos'VRj) 

which  is  zero  if  e  *  0.  For  e  «  R1  and  e  «  R2,  AC  is  clearly  small.  For  example,  taking  typical  values 
Rx  =  140  mm  and  Rj  *  180  mm  and  estimating  e  <  1  mm,  it  follows  that  AC  <  0.1°. 

If  more  precise  initialization  of  the  waist  joint  were  required,  an  additional  procedure  could  be  added 
before  starting  waist  joint  initialization: 

1.  the  shoulder  is  rotated  toward  the  work  surface  until  Che  proximity  sensor  fires; 

2.  the  elbow  is  rotate d  to  approach  the  head  of  die  target  until  any  portion  of  the  target  head  is 
encountered; 

3.  both  the  shoulder  and  elbow  are  rotated  back  through  any  specific  small  angles. 

This  procedure  reduces  the  difference  between  R2  and  Rj,  making  AC  correspondingly  very  small. 

The  shoulder  and  elbow  joint  initialization  accuracies  are  more  difficult  They  are  analyzed  below  in 
terms  of  the  number  of  initialization  process  iteration  cycles. 


2RecaB  that  wc  arc  aouminj  the  mbot  is  kinestheticatfy  precise,  and  the  wort  ’pact  is  precisely  measured. 


Figure  5:  Error  caused  by  inexact  placement  of  the  target 


4.1  Accuracy  as  a  Function  of  initialization  Cyclos 

In  this  discussion  subscript  i  refers  to  an  initial  position,  subscript  g  refers  to  a  goal  position,  and  subscript 

m  refers  to  an  intermediate  position.  First  we  suppose,  with  reference  to  Figure  3,  that  before 

initialization  the  shoulder  of  the  robot  is  at  Aj.  and  the  elbow  is  at  a  Br  Then  after  the  first  execution  of 

step  1,  the  hand  is  at  W  ,  whose  Y-coordinate  is 
r  in 

YWm  =  +  L2Sill<Bi)  +  H  =  D  (3*1) 


from  which  we  determine  that 


sin(Am)  =  (D  -  H  -  LjSU^B^/Lj  and  cosfA^  =  V  L^-fD  -  H  -  LjSmfR^)2  /Lj 
After  the  first  execution  of  step  2,  the  hand  is  at  Wg,  whose  X*  and  Y-coordinates  are: 

xwg  =  Licos(Am)  +  LjCosfB^)  =  K  and  (3*2) 

YWg  =  L1sin(Am)  +  LjSin(Bp  (3*3) 

From  Equation  (3-2)  we  get 

cos(Bg)  =  (K  -  LjCOsCA^/Lj  and  sin(Bp  =  -VL^2  -  (K  -  LjeosfA^)2  /Lj  so 
YWg  =  (D  -  H  - LjSin(B.)  +  V CJ*'  [K-  L2sin(bj5),}®Jf  +  H  (3-4) 

Equation  3-4  shows  that  Yw^  depends  only  on  B{>  not  on  A{.  Steps  3  and  4  iteratively  reduce  the 
dependence  ofYWg  on  II.  The  is  illustrated  in  Figure  6. 


In  step  3  of  the  cycle,  the  shoulder  and  elbow  are  routed  back  through  fixed  arbitrary  angles  Af  and  Bf. 


The  new  starting  position  of  the  hand,  after  one  cycle,  is  W corresponding  to  shoulder  and  elbow 
angles 

Ai(1>  =  Am  +  Ar  and  ®i (1>  =  Bg  +  Br 

After  one  additional  iteration  of  steps  1  and  2,  the  new  position  of  the  hand  is  W 
XWg(1)  =  LjeosfAj”)  +  L2cos(Bg(l>)  =  K  =  Xw# 

Yw*(1>  =  LiSin(Am(1>)  +  LjSinfB^1*)  +  H 
and  after  the  n**1  iteration,  the  position  of  the  hand  is  W  ^ 

V>  *  =  K  =  XWl 

Yw,(n>  =  Lisin(Am(0>)  +  LjSin(B^n^)  +  H 

The  explicit  solution  quickly  becomes  very  cumbersome,  but  computer  simulation  (Figure  6)  is  simple.  It 
is  interesting  to  note  that  YWg(n),  while  constant  for  given  robot  geometry,  is  not  equal  to  D. 


Figure  6:  YWg(n)(B,) 
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4.2  Relationship  Between  Accuracy  of  Initialization  an«l  Original  Elbow  Angle 


Equation  3*4  shows  that  the  accuracy  of  the  initialization  depends  on  the  original  angle  (Bp  of  the  elbow. 
Figure  7  illustrates  that  while  this  dependence  looks  strong,  the  magnitude  of  the  error  is  always  small 
This  figure  is  the  result  of  numerical  experiments  using  three  cycles,  it. 


*  -  Ov- V* 


Figure  7:  ERROR(Bj) 


4.3  Relationship  Between  Accuracy  of  tho  Initialization  and  Parameters  Br,  K,  and  0 

Equation  3-4  also  shows  that  the  accuracy  of  the  initialization  win  change  with  the  back  rotation  angle 
(Bp  of  the  elbow,  or  the  location  (K)  of  the  T-shaped  target  Figures  8,  9.  and  10  illustrate,  also  via 
numerical  experiments  involving  three  iteration  cycles,  the  errors  as  a  Auction  of  the  elbow  back  rotation 
angle  (Bp.  the  location  of  the  target  (K\  and  the  virtual  sensor  trigger  height  (D).  These  parameters  can 
thus  be  determined,  via  numerical  experiments  for  the  geometry  of  any  specific  robot  so  as  to  minimize 
the  errors  inherent  in  the  method. 

4.4  Considerations  About  Elbow  Oriontation 

As  we  mentioned  above,  as  the  shoulder  is  rotated,  the  elbow  nominally  remains  unchanged,  is,  B,  is 
constant  But  because  of  mechanical  or  calibration  imperfections,  this  capability  is  flawed: 

YW|w  =  WB,  +  A  Bp) 

Because  the  value  of  B,  is  near  270°  or  -90°  and  AB,  b  very  small,  sir(Bp  is  near  sin(&  +  AB  p.  For 
example,  suppose  Bf  s  5°,  B,  *  -73°,  AB,  =  0.1°;  then 
Ywl<2)“n(Bi  +  A  Bp  -  Yw#(5sin(Bp  =  0.6  pm 


Thus  the  error  is  negligibly  small. 
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